#include "geometry_msgs/msg/pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "robot_msgs/msg/robot_status.hpp"
#include "robot_msgs/srv/robot_move.hpp"

class DummyRobot_Node : public rclcpp::Node
{
  private:
	// robot status
	geometry_msgs::msg::Pose pose;
	unsigned int status;

	// robot status publisher
	rclcpp::Publisher<robot_msgs::msg::RobotStatus>::SharedPtr publisher;
	rclcpp::TimerBase::SharedPtr timer;

	// robot movement service
	rclcpp::Service<robot_msgs::srv::RobotMove>::SharedPtr service;

	void timer_callback(void)
	{
		robot_msgs::msg::RobotStatus msg;
		msg.set__position(pose);
		msg.set__status(status);
		publisher->publish<robot_msgs::msg::RobotStatus>(msg);
		RCLCPP_INFO(this->get_logger(), "published robot status: x(%f), y(%f), z(%f), c(%d)",
					pose.position.x, pose.position.y, pose.position.z, status);
	}

	void move_callback(const std::shared_ptr<robot_msgs::srv::RobotMove::Request> req,
					   std::shared_ptr<robot_msgs::srv::RobotMove::Response> res)
	{
		RCLCPP_INFO(this->get_logger(), "got request: x(%f), y(%f), z(%f), c(%d)",
					req->transform.translation.x, req->transform.translation.y,
					req->transform.translation.z, req->control);
		// adjust self position
		pose.position.x += req->transform.translation.x;
		pose.position.y += req->transform.translation.y;
		pose.position.z += req->transform.translation.z;
		status = req->control;
		// write variables to response
		res->position.set__position(pose.position);
		res->status = status;
	}

  public:
	DummyRobot_Node(std::string name) : Node(name)
	{
		RCLCPP_INFO(this->get_logger(), "Robot node created with name %s", name.c_str());
		publisher = this->create_publisher<robot_msgs::msg::RobotStatus>("RobotStatus", 10);
		// publish message to topic "RobotStatus" every 500 ms
		timer = this->create_wall_timer(std::chrono::milliseconds(500),
										std::bind(&DummyRobot_Node::timer_callback, this));
		// start movement service
		service = this->create_service<robot_msgs::srv::RobotMove>(
			"RobotMove", std::bind(&DummyRobot_Node::move_callback, this, std::placeholders::_1,
								   std::placeholders::_2));
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<DummyRobot_Node>("dummy_robot_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
